#include "chassic.hpp"

#include "protocol.h"
#include "spdlog/spdlog.h"
#include "verify.hpp"

#ifdef WHEEL_TEC

namespace {

#define SEND_DATA_CHECK 1  // 发送数据校验标志位
#define READ_DATA_CHECK 0  // 接收数据校验标志位

}  // namespace

inline float Combination(uint8_t high_data, uint8_t low_data) {
  float data;
  short temp;
  temp = 0;
  temp |= high_data << 8;                        // 获取数据的高8位
  temp |= low_data;                              // 获取数据的低8位
  data = (temp / 1000) + (temp % 1000) * 0.001;  // 速度单位从mm/s转换为m/s
  return data;
}

void Chassic::ThreadRecv() {
  SPDLOG_DEBUG("[ThreadRecv] Started.");

  Protocol_UpPackage_t mcu;
  uint8_t rpt[100];

  while (thread_continue) {
    auto recv_size = serial_.Recv(&rpt, sizeof(rpt));
    SPDLOG_WARN("recv_size : {}", recv_size);
    mutex_mcu_.lock();
    if (recv_size >= RECV_DATA_SIZE) {
      memcpy(mcu.rx, rpt, RECV_DATA_SIZE);
    }
    mcu.frame_header = rpt[0];
    mcu.frame_tail = rpt[RECV_DATA_SIZE - 1];
    if (bcc::BCC_Verify(rpt, RECV_DATA_SIZE, mcu.rx[22])) {
      mcu_.vx = Combination(mcu.rx[2], mcu.rx[3]);
      mcu_.vy = Combination(mcu.rx[4], mcu.rx[5]);
      mcu_.vz = Combination(mcu.rx[6], mcu.rx[7]);
      mcu_.ax = Combination(mcu.rx[8], mcu.rx[9]);
      mcu_.ay = Combination(mcu.rx[10], mcu.rx[11]);
      mcu_.az = Combination(mcu.rx[12], mcu.rx[13]);
      mcu_.wx = Combination(mcu.rx[14], mcu.rx[15]);
      mcu_.wy = Combination(mcu.rx[16], mcu.rx[17]);
      mcu_.wz = Combination(mcu.rx[18], mcu.rx[19]);
      mcu_.power_voltage = Combination(mcu.rx[20], mcu.rx[21]);
    }

    // mcu_.ax = mcu.data.ax;
    // mcu_.ay = mcu.data.ay;
    // mcu_.az = mcu.data.az;
    // mcu_.wx = mcu.data.wx;
    // mcu_.wy = mcu.data.wy;
    // mcu_.wz = mcu.data.wz;
    // mcu_.vx = mcu.data.vx;
    // mcu_.vy = mcu.data.vy;
    // mcu_.vz = mcu.data.vz;
    // mcu_.power_voltage = mcu.data.power_voltage;
    mutex_mcu_.unlock();
  }

  SPDLOG_DEBUG("[ThreadRecv] Stoped.");
}

void Chassic::ThreadTrans() {
  SPDLOG_DEBUG("[ThreadTrans] Started.");

  Protocol_DownPackage_t command;

  while (thread_continue) {
    pack_signal_.Wait();
    bool is_empty = true;
    mutex_command_.lock();
    if (commandq_.size() > 0) {
      command = commandq_.front();
      commandq_.pop_front();
      is_empty = false;
    }
    mutex_command_.unlock();
    if (!is_empty) {
      serial_.Trans((char *)&command, sizeof(command));
      //   if (serial_.Trans((char *)&command, sizeof(command))) {
      //     mutex_command_.lock();
      //     while (!serial_.Reopen())
      //       std::this_thread::sleep_for(std::chrono::milliseconds(1));
      //     mutex_command_.unlock();
      //   }
    }
  }
  SPDLOG_DEBUG("[ThreadTrans] Stoped.");
}

Chassic::Chassic() { SPDLOG_TRACE("Constructed."); }

Chassic::Chassic(const std::string &dev_path) {
  Init(dev_path);

  SPDLOG_TRACE("Constructed.");
}

Chassic::~Chassic() {
  serial_.Close();

  thread_continue = false;
  thread_recv_.join();
  thread_trans_.join();
  SPDLOG_TRACE("Destructed.");
}

void Chassic::Init(const std::string &dev_path) {
  serial_.Open(dev_path);

  thread_continue = true;
  thread_recv_ = std::thread(&Chassic::ThreadRecv, this);
  thread_trans_ = std::thread(&Chassic::ThreadTrans, this);
}

void Chassic::Pack(component::Velocity velocity) {
  Protocol_DownPackage_t pack;
  mutex_command_.lock();

  pack.tx[0] = bcc::FRAME_HEADER;
  pack.tx[1] = 0;
  pack.tx[2] = 0;

  short temp = 0;
  temp = velocity.vx * 1000;
  pack.tx[4] = temp;
  pack.tx[3] = temp >> 8;

  temp = 0;
  temp = velocity.vy * 1000;
  pack.tx[6] = temp;
  pack.tx[5] = temp >> 8;

  temp = 0;
  temp = velocity.vy * 1000;
  pack.tx[8] = temp;
  pack.tx[7] = temp >> 8;

  pack.tx[9] = bcc::BCC_Calc(pack.tx, TRANS_DATA_SIZE - 1);
  pack.tx[10] = bcc::FRAME_TAIL;

  pack.data = velocity.Convert();

  commandq_.emplace_back(pack);
  pack_signal_.Signal();
  mutex_command_.unlock();
}

float Chassic::GetVel() { return mcu_.vx; }

#else

void Chassic::ThreadRecv() {
  SPDLOG_DEBUG("[ThreadRecv] Started.");
  Protocol_UpPackage_t robot;

  while (thread_continue) {
    serial_.Recv(&robot, sizeof(robot));
    if (crc16::CRC16_Verify((uint8_t *)&robot, sizeof(robot))) {
      mutex_mcu_.lock();
      // recorder_.Record();
      std::memcpy(&mcu_, &(robot.data), sizeof(mcu_));
      mutex_mcu_.unlock();
    }
  }
  SPDLOG_DEBUG("[ThreadRecv] Stoped.");
}

void Chassic::ThreadTrans() {
  SPDLOG_DEBUG("[ThreadTrans] Started.");

  Protocol_DownPackage_t command;

  while (thread_continue) {
    pack_signal_.Wait();
    bool is_empty = true;
    mutex_command_.lock();
    if (commandq_.size() > 0) {
      SPDLOG_WARN("GetData");
      command.data = commandq_.front();
      commandq_.pop_front();
      is_empty = false;
    }
    mutex_command_.unlock();
    if (!is_empty) {
      command.crc16 = crc16::CRC16_Calc((uint8_t *)&command.data,
                                        sizeof(command.data), UINT16_MAX);
      serial_.Trans((char *)&command, sizeof(command));
      SPDLOG_INFO("Send");
      // if (serial_.Trans((char *)&command, sizeof(command))) {
      //   mutex_command_.lock();
      //   while (!serial_.Reopen())
      //     std::this_thread::sleep_for(std::chrono::milliseconds(1));
      //   mutex_command_.unlock();
      // }
    }
  }
  SPDLOG_DEBUG("[ThreadTrans] Stoped.");
}

Chassic::Chassic() { SPDLOG_TRACE("Constructed."); }

Chassic::Chassic(const std::string &dev_path) {
  Init(dev_path);
  SPDLOG_TRACE("Constructed.");
}

Chassic::~Chassic() {
  serial_.Close();

  thread_continue = false;
  thread_recv_.join();
  thread_trans_.join();
  SPDLOG_TRACE("Destructed.");
}

void Chassic::Init(const std::string &dev_path) {
  serial_.Open(dev_path);

  thread_continue = true;
  thread_recv_ = std::thread(&Chassic::ThreadRecv, this);
  thread_trans_ = std::thread(&Chassic::ThreadTrans, this);
}

void Chassic::Pack(component::Velocity velocity) {
  Protocol_DownData_t data;
  data.vx = velocity.vx;
  data.vy = velocity.vy;
  data.wz = velocity.wz;

  mutex_command_.lock();
  commandq_.emplace_back(data);
  pack_signal_.Signal();
  mutex_command_.unlock();
}

Protocol_UpData_t Chassic::GetMCU() { return mcu_; }

// float Chassic::GetAverMotor() const {
//   return ((mcu_.motors.m1 + mcu_.motors.m2 + mcu_.motors.m3 + mcu_.motors.m4)
//   /
//           4);
// }

#endif

////////////////////////////////////
//
// Android
//
////////////////////////////////////

// void Android::ThreadRecv() {
//   SPDLOG_DEBUG("[ThreadRecv] Started.");
//   Protocol_UpPackage_t robot;

//   while (thread_continue) {
//     serial_.Recv(&robot, sizeof(robot));
//     if (crc16::CRC16_Verify((uint8_t *)&robot, sizeof(robot))) {
//       mutex_andr_.lock();
//       // recorder_.Record();
//       std::memcpy(&vel_, &(robot.data), sizeof(vel_));
//       mutex_andr_.unlock();
//     }
//   }
//   SPDLOG_DEBUG("[ThreadRecv] Stoped.");
// }

void Android::ThreadTrans() {
  SPDLOG_DEBUG("[ThreadTrans] Started.");

  // Protocol_DownPackage_t command;
  std::vector<uint8_t> data;

  while (thread_continue) {
    data.clear();
    pack_signal_.Wait();
    bool is_empty = true;
    mutex_lidar_.lock();
    if (lidars_.size() > 0) {
      SPDLOG_WARN("GetData");
      data = lidars_.front();
      lidars_.pop_front();
      is_empty = false;
    }
    mutex_lidar_.unlock();
    if (!is_empty) {
      serial_.Trans((char *)&data, sizeof(data));
      // SPDLOG_INFO("Send");
      // if (serial_.Trans((char *)&command, sizeof(command))) {
      //   mutex_command_.lock();
      //   while (!serial_.Reopen())
      //     std::this_thread::sleep_for(std::chrono::milliseconds(1));
      //   mutex_command_.unlock();
      // }
    }
  }
  SPDLOG_DEBUG("[ThreadTrans] Stoped.");
}

Android::Android() { SPDLOG_TRACE("Constructed."); }

Android::Android(const std::string &dev_path) {
  Init(dev_path);
  SPDLOG_TRACE("Constructed.");
}

Android::~Android() {
  serial_.Close();

  thread_continue = false;
  // thread_recv_.join();
  thread_trans_.join();
  SPDLOG_TRACE("Destructed.");
}

void Android::Init(const std::string &dev_path) {
  serial_.Open(dev_path);

  thread_continue = true;
  // thread_recv_ = std::thread(&Android::ThreadRecv, this);
  thread_trans_ = std::thread(&Android::ThreadTrans, this);
}

void Android::Pack(std::vector<uint8_t> data) {
  // Protocal_Andr data;
  // data.vx = velocity.vx;
  // data.vy = velocity.vy;
  // data.wz = velocity.wz;
  if (data.size() == 0) return;
  mutex_lidar_.lock();
  lidars_.emplace_back(data);
  pack_signal_.Signal();
  mutex_lidar_.unlock();
}
